clc;
clear;
close all;

m = 176.3; % t
g = 9.81;

v = 0:0.1:28; %m/s
n = length(v);

global eta_t;
eta_t = 1;

% Ft = zeros(n,1);
% Fb = zeros(n,1);
% w0 = zeros(n,1); % kN
% for i = 1:n
%     Ft(i) = eta_t*min([310,310*10/v(i)]);
%     Fb(i) = eta_t*min([260,260*17/v(i)]);
%     w0(i) = (2.0895 + 0.0098*v(i) +0.0065*v(i)^2)*m*g/1000;
% end
% 
% figure(1)
% plot(v,Ft,v,Fb,v,w0);
% title('Original Force Line');
% xlabel('v [m/s]');
% ylabel('F [kN]')

v_ = 1:350;
for v=1:350
    F(v) = getMaxFt(v);
    w0(v) = cal_unit_basicResist([0.55 0.003622 0.00011], v).*425*9.81/1000;
    Fb(v) = -getMaxFeb(v);
    lastf(v) = F(v)-w0(v);
end
Ft = F;
figure(11)
% subplot(2,2,1);
plot(v_,Ft,'LineWidth',2);
hold on;
plot(v_,w0,'LineWidth',2);
plot(v_,lastf,'LineWidth',2);
plot(v_,Fb,'LineWidth',2);
plot([0,350],[0,0],'LineWidth',1,'LineStyle','--','Color',[0 0 0]);
axis([0 350 -300 300]);
xlabel('v [km/h]');
ylabel('F [kN]');
legend("牵引特性","基本阻力","剩余牵引力","电制动力");

%% 利用GA确定线性近似线性函数参数
% 设置遗传算法的参数
options = optimoptions('ga', 'MaxGenerations', 1000, 'PopulationSize', 100);

% 拟合牵引特性曲线
lb = [300 -5]; % 参数下界
ub = [700 0]; % 参数上界
fitnessFunctionHandle = @(ab) fitnessFunction1(ab, v_);
[ab_ft, fval_ft] = ga(fitnessFunctionHandle, 2, [], [], [], [], lb, ub, [], options);

% 拟合制动特性曲线
lb = [260 -1]; % 参数下界
ub = [500 0]; % 参数上界
fitnessFunctionHandle = @(ab) fitnessFunction2(ab, v_);
[ab_fb, fval_fb] = ga(fitnessFunctionHandle, 2, [], [], [], [], lb, ub, [], options);

% 拟合基本阻力特性曲线 分段
% lb = [-10  0 -100 0.13]; % 参数下界
% ub = [ 10 0.13 0  0.3]; % 参数上界
% fitnessFunctionHandle = @(abcd) fitnessFunction3(abcd, v_);
% [ab_w0, fval_w0] = ga(fitnessFunctionHandle, 4, [], [], [], [], lb, ub, [], options);

% 拟合基本阻力特性曲线 2参数
lb = [-100  0]; % 参数下界
ub = [ 10 1]; % 参数上界
fitnessFunctionHandle = @(ab) fitnessFunction3_2(ab, v_);
[ab_w0, fval_w0] = ga(fitnessFunctionHandle, 2, [], [], [], [], lb, ub, [], options);

% % 拟合牵引力特性曲线 （a + bv + cv^2）
% lb = [200 -20 -1]; % 参数下界 a + bv + cv^2
% ub = [500 0 0]; % 参数上界
% fitnessFunctionHandle = @(abc) fitnessFunction_abc(abc, v);
% [abc_ft, fval_ft_abc] = ga(fitnessFunctionHandle, 3, [], [], [], [], lb, ub, [], options);
% 
% % 拟合电制动特性曲线 （a + bv + cv^2）
% lb = [200 -10 -2]; % 参数下界 a + bv + cv^2
% ub = [500 1 0]; % 参数上界
% fitnessFunctionHandle = @(abc) fitnessFunction_abc2(abc, v);
% [abc_fd, fval_fd_abc] = ga(fitnessFunctionHandle, 3, [], [], [], [], lb, ub, [], options);

%% compare 
v = v_;
for i = 1:length(v)
    Ft_hat(i) = min([-0.375*i+276, ab_ft(1)+ab_ft(2)*v(i)]);
    Fb_hat(i) = -min([27.2*i, -0.08667*i+272.9, ab_fb(1)+ab_fb(2)*v(i)]);
    % w0_hat(i) = max([ab_w0(1)+ab_w0(2)*v(i), ab_w0(3)+ab_w0(4)*v(i)]);
    w0_hat(i) = (ab_w0(1)+ab_w0(2)*v(i))*425*9.81/1000;
end
figure(2)
plot(v,Ft,'b',"LineWidth",2);
hold on;
plot(v,Ft_hat,'b--',"LineWidth",2);
plot(v,Fb,'r',"LineWidth",2);
plot(v,Fb_hat,'r--',"LineWidth",2);
plot(v,w0,'g',"LineWidth",2);
plot(v,w0_hat,'m--',"LineWidth",2);
legend("Ft","fit-Ft","Fd","fit-Fd","W0","fit-W0");
grid on;

% plot(v,ab_t);

% Ft_hat_abc = zeros(n,1);
% Fd_hat_abc = zeros(n,1);
% for i = 1:n
%     Ft_hat_abc(i) = min([310, abc_ft(1)+abc_ft(2)*v(i)+abc_ft(3)*v(i)^2]);
% 
%     Fd_hat_abc(i) = min([260, abc_fd(1)+abc_fd(2)*v(i)+abc_fd(3)*v(i)^2]);
% end
% figure(3)
% plot(v,Ft,'b',"LineWidth",2);
% hold on;
% plot(v,Ft_hat_abc,'b--',"LineWidth",2);
% plot(v,Fb,'r',"LineWidth",2);
% plot(v,Fd_hat_abc,'r--',"LineWidth",2);
% legend("Ft","fit-Ft","Fd","fit-Fd");
% grid on;

%% functions
% 定义牵引特性曲线适应度函数
function error = fitnessFunction1(ab,v)
global eta_t;
n = length(v);
F = zeros(n,1);
F_hat = zeros(n,1);
for i = 1:n
    F(i) = getMaxFt(i);
    F_hat(i) = min([-0.375*i+276, ab(1)+ab(2)*v(i)]); % a+bv
end
error = norm(F - F_hat);
end

% 定义电制动特性曲线适应度函数
function error = fitnessFunction2(ab,v)
global eta_t;
n = length(v);
F = zeros(n,1);
F_hat = zeros(n,1);
for i = 1:n
    F(i) = getMaxFeb(i);
    F_hat(i) = min([27.2*i, -0.08667*i+272.9, ab(1)+ab(2)*v(i)]); % a+bv^2
end
error = norm(F - F_hat);
end

% 定义基本阻力曲线适应度函数
function error = fitnessFunction3(abcd,v)
n = length(v);
F = zeros(n,1);
F_hat = zeros(n,1);
for i = 1:n
    F(i) = cal_unit_basicResist([0.55 0.003622 0.00011], v(i))*425*9.81/1000;
    F_hat(i) = max([abcd(1)+abcd(2)*v(i), abcd(3)+abcd(4)*v(i)])*425*9.81/1000; % a+bv
end
error = norm(F - F_hat);
end

function error = fitnessFunction3_2(ab,v)
n = length(v);
F = zeros(n,1);
F_hat = zeros(n,1);
for i = 1:n
    F(i) = cal_unit_basicResist([0.55 0.003622 0.00011], v(i))*425*9.81/1000;
    F_hat(i) = (ab(1)+ab(2)*v(i))*425*9.81/1000; % a+bv
end
error = norm(F - F_hat);
end

% 尝试用a+bv+cv^2去逼近牵引特性曲线
function error = fitnessFunction_abc(abc,v)
n = length(v);
F = zeros(n,1);
F_hat = zeros(n,1);
for i = 1:n
    F(i) = min([310,310*10/v(i)]); % 牵引特性外包络
    F_hat(i) = min([310, abc(1)+abc(2)*v(i)+abc(3)*v(i)^2]); % a + bv + cv^2
end
error = norm(F - F_hat);
end

% 尝试用a+bv+cv^2去逼近电制动特性曲线
function error = fitnessFunction_abc2(abc,v)
n = length(v);
F = zeros(n,1);
F_hat = zeros(n,1);
for i = 1:n
    F(i) = min([260,260*17/v(i)]); % 电制动特性外包络
    F_hat(i) = min([260, abc(1)+abc(2)*v(i)+abc(3)*v(i)^2]); % a + bv + cv^2
end
error = norm(F - F_hat);
end